package brainbots.bot;

import brainbots.Food;
import brainbots.ItemEnumerator;
import brainbots.Located;
import brainbots.World;
import brainbots.brains.Brain;
import brainbots.util.BBUtil;
import brainbots.util.Rotation;


public class BrainBot extends Located implements Body {
	/**maximal value of food energy*/
    private static final double MAX_ENERGY = 100;
	/**Coordinates and speed*/
    private double[] vxy=new double[2];
    private World world;
    
    /**Rotation and rotation speed*/
    public Rotation phi = null;
    private double dphi; ///rotation speed
    
    /**Food energy*/
    private double energy=MAX_ENERGY;
    
    /**Dynamics parameters*/
    double mass, inertia_momentum;
    double movement_friction, rotation_friction;
    
    
    /**Motor parameters*/
    Motor[] motors;
    Sensor[] sensors;
    
    private Brain brain=null;
	private double energyConsumptionRate=0.1;
	private double idleEnergyConsumptionRate = 0.3;
    
    /**Return food energy value*/
    public double getEnergy() {
		return energy;
	}
    
    /**Set object that controls bot behavior*/
    public void setBrain(Brain b){
        brain = b;
    }
    
    public final int getMotorCount(){
        return motors.length;
    }

    public final Motor getMotor(int idx){
        return motors[idx];
    }
    public BrainBot(double x, double y, double angle){
    	super(x,y);
        this.phi = new Rotation(angle);
        vxy[0]=0;
        vxy[1]=0;
        dphi=0;
        
        energy = 100;
        
        initMotors();
        initSensors();
        initParameters();
        
        world = null;//will be set when bot is attached to the world
    }
    public void setEnergy(double energy) {
		this.energy = energy;
	}
    
    public void setWorld(World w1){
        //attach bot to the anouther world
        world = w1;
    }
    
    protected void initParameters()
    {
        mass=1.0;
        inertia_momentum = 10.0;
        movement_friction = 0.1;
        rotation_friction = 1.5;
    }
    
    /**Controlling motor*/
    public final void setMotor(int idx, double value)
    {
        motors[idx].set(value);
    }
    
    private void initSensors(){
        sensors = new Sensor[2];
        sensors[0] = new FoodSensor(-0.5, 0.5, Math.PI*3/4);
        sensors[1] = new FoodSensor( 0.5, 0.5, Math.PI*1/4);
    }
    private void initMotors(){
        motors = new Motor[4];
        motors[0] = new Motor(0,1,-1,0);
        motors[1] = new Motor(0,1, 1,0);
        
        motors[2] = new Motor( 1,-1,0,1);
        motors[3] = new Motor(-1,-1,0,1);
        
    }
    
    /**SImulate bot movement*/
    public void simulate (double dt){
        //TODO
    	shift(vxy[0]*dt,vxy[1]*dt);
        
    
        //Rotation
        phi.add(dphi*dt);
        
        //Motors
        simMotors(dt);
        
        //Friciotn effects
        simFriction(dt);
        
        simSensors();
        
        //brains
        simBrain(dt);
        
        //food-eating behavior
        eatFood(); //TODO move it to the body?
    }
    
    private void simSensors() {
		for(int i =0;i<sensors.length;++i){
			sensors[i].perceive(world, this);
		}
	}

	private void simMotors(double dt){
        double[] f=new double[]{0.0,0.0};
        
        double fr=0; //rotational force
        double de = idleEnergyConsumptionRate; //energy consumption
        
        for (int i =0;i< motors.length; ++i){
            Motor m = motors[i];
            m.simulate(dt);//simulate the internals of the motor first
            //apply motor force to the bot
            m.addForce(f, phi); //apply movement
            de += m.getEnergyConsumption();
            fr += m.getMoment(); //apply rotational moment
        }
        
        BBUtil.add(vxy,f,dt/mass);
        dphi += fr/inertia_momentum*dt;
        energy -= de*dt*energyConsumptionRate;
    }
    
    /**Simulate friction effect*/
    private void simFriction(double dt)
    {
        double v2 = BBUtil.sqr(vxy[0])+BBUtil.sqr(vxy[1]);
        
        BBUtil.add(vxy, vxy, -dt*movement_friction*Math.sqrt(v2));
        
        dphi += -dt*dphi*Math.abs(dphi)*rotation_friction;
    }
        
        
    protected void simBrain(double dt){
        if (brain != null){
            brain.simulate(dt, this);
        }
    }
    
    private Food foodToEat;
    /**Simulate process of food eating*/
    private void eatFood(){
        //Find all food nearby and eat it
    	foodToEat = null;
    	
        world.foodCells.inCircle(getX(), getY(),5.0, new ItemEnumerator<Food>(){
            public boolean item(Food f){
            	foodToEat = f;
            	return true;
            }
        });
        if (foodToEat != null){
        	//found something to eat!
        	
//            System.out.println("ate food piece:"+this+":  "+foodToEat);
        	energy += foodToEat.getEnergy();
        	if (energy >= MAX_ENERGY){
        		energy = MAX_ENERGY;
        	}
        	world.eatFood(this, foodToEat);
        }
    }
    
/**Getters for coordinates*/
    public final double getVX(){
        return vxy[0];
    }
    public final double getVY(){
        return vxy[1];
    }
    public final void setVX(double vx){
        vxy[0] = vx;
    }
    public final void setVY(double vy){
        vxy[1] = vy;
    }

    public final Rotation getPhi(){
    	return phi;
    }

	@Override
	public int getSensorCount() {
		return sensors.length;
	}

	@Override
	public double getSensor(int idx) {
		return sensors[idx].getValue();
	}

	public Brain getBrain() {
		return brain;
	}

	/**Decrease value of the enegy by the given amount*/
	public void decreaseEnergy(int amount) {
		energy -= amount;
	}
	
	public BotColors colors;

}